#include "foc.h"

void inverse_clarke(float alpha, float beta, float *a, float *b, float *c)
{
  *a =  0.6667f * alpha;
  *b = -0.3333f * alpha + 0.5774f * beta; // 0.8660254 = sqrt(3)/2
  *c = -0.3333f * alpha - 0.5774f * beta;
}

void inverse_park(float d, float q, float theta, float *alpha, float *beta)
{
  float sin_val = sinf(theta);
  float cos_val = cosf(theta);

  *alpha = d * cos_val - q * sin_val;
  *beta  = d * sin_val + q * cos_val;
}

float zero_processing(float target,float fb,float midfb)
{
	float e = target - fb;
	if(e > midfb)
	{
		return fb + 360;
	}
	else if(e < -midfb)
	{
		return fb - 360;
	}
	return fb;
}
